;=================================================================================
;=========== SOME FUNCTION FOR MATRIX 4x4
;=================================================================================
Structure iMATRIX
  m.f[16]
EndStructure

Procedure Matrix_Identity(*mat.iMATRIX)
  *mat\m[0]=0 :  *mat\m[1]=0  :  *mat\m[2]=0  :  *mat\m[3]=1
  *mat\m[4]=0 :  *mat\m[5]=0  :  *mat\m[6]=0  :  *mat\m[7]=1
  *mat\m[8]=0 :  *mat\m[9]=0  :  *mat\m[10]=0 :  *mat\m[11]=1
  *mat\m[12]=0:  *mat\m[13]=0 :  *mat\m[14]=0 :  *mat\m[15]=1
EndProcedure

Procedure Matrix_Equals(*des.iMATRIX, *m1.iMATRIX)
  *des\m[0]   = *m1\m[0]
  *des\m[1]   = *m1\m[1]
  *des\m[2]   = *m1\m[2]
  *des\m[3]   = *m1\m[3]
  *des\m[4]   = *m1\m[4]
  *des\m[5]   = *m1\m[5]
  *des\m[6]   = *m1\m[6]
  *des\m[7]   = *m1\m[7] 
  *des\m[8]   = *m1\m[8]
  *des\m[9]   = *m1\m[9]
  *des\m[10]  = *m1\m[10]
  *des\m[11]  = *m1\m[11]
  *des\m[12]  = *m1\m[12]
  *des\m[13]  = *m1\m[13] 
  *des\m[14]  = *m1\m[14]
  *des\m[15]  = *m1\m[15]
EndProcedure

Procedure Matrix_Add(*des.iMATRIX, *m1.iMATRIX, *m2.iMATRIX)
  *des\m[0]   = *m1\m[0]  +   *m2\m[0]
  *des\m[1]   = *m1\m[1]  +   *m2\m[1]
  *des\m[2]   = *m1\m[2]  +   *m2\m[2]
  *des\m[3]   = *m1\m[3]  +   *m2\m[3]
  *des\m[4]   = *m1\m[4]  +   *m2\m[4]
  *des\m[5]   = *m1\m[5]  +   *m2\m[5]
  *des\m[6]   = *m1\m[6]  +   *m2\m[6]
  *des\m[7]   = *m1\m[7]  +   *m2\m[7]
  *des\m[8]   = *m1\m[8]  +   *m2\m[8]
  *des\m[9]   = *m1\m[9]  +   *m2\m[9]
  *des\m[10]  = *m1\m[10]  +   *m2\m[10]
  *des\m[11]  = *m1\m[11]  +   *m2\m[11]
  *des\m[12]  = *m1\m[12]  +   *m2\m[12]
  *des\m[13]  = *m1\m[13]  +   *m2\m[13]
  *des\m[14]  = *m1\m[14]  +   *m2\m[14]
  *des\m[15]  = *m1\m[15]  +   *m2\m[15]
EndProcedure

Procedure Matrix_Sub(*des.iMATRIX, *m1.iMATRIX, *m2.iMATRIX)
  *des\m[0]   = *m1\m[0]  -   *m2\m[0]
  *des\m[1]   = *m1\m[1]  -   *m2\m[1]
  *des\m[2]   = *m1\m[2]  -   *m2\m[2]
  *des\m[3]   = *m1\m[3]  -   *m2\m[3]
  *des\m[4]   = *m1\m[4]  -   *m2\m[4]
  *des\m[5]   = *m1\m[5]  -   *m2\m[5]
  *des\m[6]   = *m1\m[6]  -   *m2\m[6]
  *des\m[7]   = *m1\m[7]  -   *m2\m[7]
  *des\m[8]   = *m1\m[8]  -   *m2\m[8]
  *des\m[9]   = *m1\m[9]  -   *m2\m[9]
  *des\m[10]  = *m1\m[10]  -   *m2\m[10]
  *des\m[11]  = *m1\m[11]  -   *m2\m[11]
  *des\m[12]  = *m1\m[12]  -   *m2\m[12]
  *des\m[13]  = *m1\m[13]  -   *m2\m[13]
  *des\m[14]  = *m1\m[14]  -   *m2\m[14]
  *des\m[15]  = *m1\m[15]  -   *m2\m[15]
EndProcedure

Procedure Matrix_MulF(*des.iMATRIX, *m1.iMATRIX, scalar.f)
  *des\m[0]   = *m1\m[0]  * scalar 
  *des\m[1]   = *m1\m[1]  * scalar 
  *des\m[2]   = *m1\m[2]  * scalar 
  *des\m[3]   = *m1\m[3]  * scalar 
  *des\m[4]   = *m1\m[4]  * scalar 
  *des\m[5]   = *m1\m[5]  * scalar 
  *des\m[6]   = *m1\m[6]  * scalar 
  *des\m[7]   = *m1\m[7]  * scalar 
  *des\m[8]   = *m1\m[8]  * scalar 
  *des\m[9]   = *m1\m[9]  * scalar 
  *des\m[10]  = *m1\m[10]  * scalar 
  *des\m[11]  = *m1\m[11]  * scalar 
  *des\m[12]  = *m1\m[12]  * scalar 
  *des\m[13]  = *m1\m[13]  * scalar 
  *des\m[14]  = *m1\m[14]  * scalar 
  *des\m[15]  = *m1\m[15]  * scalar 
EndProcedure

Procedure Matrix_Mul(*m3.iMATRIX, *m1.iMATRIX, *m2.iMATRIX)
  Protected mm.iMATRIX
 	Matrix_Equals( @mm, *m1)

		*m3\m[0] = mm\m[0]* *m2\m[0] + mm\m[4]* *m2\m[1] + mm\m[8]* *m2\m[2] + mm\m[12]* *m2\m[3];
		*m3\m[1] = mm\m[1]* *m2\m[0] + mm\m[5]* *m2\m[1] + mm\m[9]* *m2\m[2] + mm\m[13]* *m2\m[3];
		*m3\m[2] = mm\m[2]* *m2\m[0] + mm\m[6]* *m2\m[1] + mm\m[10]* *m2\m[2] + mm\m[14]* *m2\m[3];
		*m3\m[3] = mm\m[3]* *m2\m[0] + mm\m[7]* *m2\m[1] + mm\m[11]* *m2\m[2] + mm\m[15]* *m2\m[3];

		*m3\m[4] = mm\m[0]* *m2\m[4] + mm\m[4]* *m2\m[5] + mm\m[8]* *m2\m[6] + mm\m[12]* *m2\m[7];
		*m3\m[5] = mm\m[1]* *m2\m[4] + mm\m[5]* *m2\m[5] + mm\m[9]* *m2\m[6] + mm\m[13]* *m2\m[7];
		*m3\m[6] = mm\m[2]* *m2\m[4] + mm\m[6]* *m2\m[5] + mm\m[10]* *m2\m[6] + mm\m[14]* *m2\m[7];
		*m3\m[7] = mm\m[3]* *m2\m[4] + mm\m[7]* *m2\m[5] + mm\m[11]* *m2\m[6] + mm\m[15]* *m2\m[7];

		*m3\m[8] = mm\m[0]* *m2\m[8] + mm\m[4]* *m2\m[9] + mm\m[8]* *m2\m[10] + mm\m[12]* *m2\m[11];
		*m3\m[9] = mm\m[1]* *m2\m[8] + mm\m[5]* *m2\m[9] + mm\m[9]* *m2\m[10] + mm\m[13]* *m2\m[11];
		*m3\m[10] = mm\m[2]* *m2\m[8] + mm\m[6]* *m2\m[9] + mm\m[10]* *m2\m[10] + mm\m[14]* *m2\m[11];
		*m3\m[11] = mm\m[3]* *m2\m[8] + mm\m[7]* *m2\m[9] + mm\m[11]* *m2\m[10] + mm\m[15]* *m2\m[11];

		*m3\m[12] = mm\m[0]* *m2\m[12] + mm\m[4]* *m2\m[13] + mm\m[8]* *m2\m[14] + mm\m[12]* *m2\m[15];
		*m3\m[13] = mm\m[1]* *m2\m[12] + mm\m[5]* *m2\m[13] + mm\m[9]* *m2\m[14] + mm\m[13]* *m2\m[15];
		*m3\m[14] = mm\m[2]* *m2\m[12] + mm\m[6]* *m2\m[13] + mm\m[10]* *m2\m[14] + mm\m[14]* *m2\m[15];
		*m3\m[15] = mm\m[3]* *m2\m[12] + mm\m[7]* *m2\m[13] + mm\m[11]* *m2\m[14] + mm\m[15]* *m2\m[15];
EndProcedure

Procedure Matrix_GetTranslation(*des.iVECTOR3, *m.iMATRIX)
  *des\x = *m\m[12]
  *des\y = *m\m[13]
  *des\z = *m\m[14]
EndProcedure

Procedure Matrix_SetTranslation(*m.iMATRIX, *des.iVECTOR3)
  *m\m[12] = *des\x
  *m\m[13] = *des\y
  *m\m[14] = *des\z
EndProcedure

Procedure Matrix_GetScale(*des.iVECTOR3, *m.iMATRIX)
  *des\x = *m\m[0]
  *des\y = *m\m[5]
  *des\z = *m\m[10]
EndProcedure

Procedure Matrix_SetScale(*m.iMATRIX, *des.iVECTOR3)
  *m\m[0] = *des\x
  *m\m[5] = *des\y
  *m\m[10] = *des\z
EndProcedure

; in Radian
Procedure Matrix_SetRotation(*M.iMATRIX, *rotation.iVECTOR3)
    Protected cr.f, sr.f, cp.f,	sp.f,	cy.f,	sy.f
    Protected srsp.f, crsp.f
    
		cr = Cos( *rotation\x )
		sr = Sin( *rotation\x )
		cp = Cos( *rotation\y )
		sp = Sin( *rotation\y )
		cy = Cos( *rotation\z )
		sy = Sin( *rotation\z )

		*M\m[0] = cp*cy
		*M\m[1] = cp*sy
		*M\m[2] = -sp

		srsp = sr*sp
		crsp = cr*sp

		*M\m[4] = srsp*cy-cr*sy
		*M\m[5] = srsp*sy+cr*cy
		*M\m[6] = sr*cp

		*M\m[8] =  crsp*cy+sr*sy
		*M\m[9] =  crsp*sy-sr*cy
		*M\m[10] = cr*cp
		
EndProcedure

; in degres
Procedure Matrix_SetRotationDegrees(*M.iMATRIX, *rotation.iVECTOR3)
  Protected rot.iVECTOR3
  rot\x = DEG2RAD( *rotation\x )
  rot\y = DEG2RAD( *rotation\y )
  rot\z = DEG2RAD( *rotation\z )
  Matrix_SetRotation(*M, @rot)
EndProcedure

Procedure Matrix_GetRotation(*rotation.iVECTOR3, *M.iMATRIX)
		Protected mat.iMATRIX
		Protected rotx.f, roty.f, X.f, Z.f, C.f, Y.f
		Protected invC.f
		
		Matrix_Equals(@mat, *M)

		Y = -ASin( mat\m[2])
		C = Cos(Y)
		Y = RAD2DEG(Y)


		If ( Abs(C) > ROUNDING_ERROR_64)
		
			invC = 1.0/C
			rotx = mat\m[10] * invC
			roty = mat\m[6] * invC
			X = atan2( roty, rotx ) :  X = RAD2DEG(X)
			rotx = mat\m[0] * invC
			roty = mat\m[1] * invC
			Z = atan2( roty, rotx ) : Z = RAD2DEG(Z)
		
		Else
		
			X = 0.0
			rotx = mat\m[5]
			roty = -mat\m[4]
			Z = atan2( roty, rotx ) : Z = RAD2DEG(Z)
		EndIf

		; fix values that get below zero
		; before it would set (!) values To 360
		; that where above 360:
		If (X < 0.0) : X + 360.0 : EndIf
		If (Y < 0.0) : Y + 360.0 : EndIf
		If (Z < 0.0) : Z + 360.0 : EndIf

		*rotation\x = X
		*rotation\y = Y
		*rotation\z = ZX

EndProcedure

Procedure Matrix_RotateVect(*M.iMATRIX, *rotation.iVECTOR3)
		Protected tmp.iVECTOR3
		Vec3_Equals(@tmp, *rotation)

		*rotation\X = tmp\X* *M\m[0] + tmp\Y* *M\m[4] + tmp\Z* *M\m[8]
		*rotation\Y = tmp\X* *M\m[1] + tmp\Y* *M\m[5] + tmp\Z* *M\m[9]
		*rotation\Z = tmp\X* *M\m[2] + tmp\Y* *M\m[6] + tmp\Z* *M\m[10]
EndProcedure

Procedure Matrix_TransformVect(*vect.iVECTOR3, *M.iMATRIX )

		Protected vector.iVECTOR3

		vector\x = *vect\X* *M\m[0] + *vect\Y* *M\m[4] + *vect\Z* *M\m[8] + *M\m[12]
		vector\y = *vect\X* *M\m[1] + *vect\Y* *M\m[5] + *vect\Z* *M\m[9] + *M\m[13]
		vector\z = *vect\X* *M\m[2] + *vect\Y* *M\m[6] + *vect\Z* *M\m[10] + *M\m[14]

		*vect\X = vector\x
		*vect\Y = vector\y
		*vect\Z = vector\z
EndProcedure

Procedure Matrix_TransformVectV(*out.iVECTOR3, *in.iVECTOR3, *M.iMATRIX )
 		*out\X = *in\X* *M\m[0] + *in\Y* *M\m[4] + *in\Z* *M\m[8] +  *M\m[12]
 		*out\Y = *in\X* *M\m[1] + *in\Y* *M\m[5] + *in\Z* *M\m[9] +  *M\m[13]
 		*out\Z = *in\X* *M\m[2] + *in\Y* *M\m[6] + *in\Z* *M\m[10] + *M\m[14]
EndProcedure


Procedure Matrix_TransformPlane( *plane.iPLANE, *M.iMATRIX)
 
 		Protected member.iVECTOR3, normal.iVECTOR3, scale.iVECTOR3
 		
    ; Fully transform the plane member point, i.e. rotate, translate And scale it.
    Matrix_TransformVectV(@member , @*plane\Normal, *M ) 
 		normal\x = *plane\Normal\x
 		normal\y = *plane\Normal\y
 		normal\z = *plane\Normal\z

    Vec3_Normalize(@normal)
 
    ; The normal needs To be rotated And inverse scaled, but Not translated.
    Matrix_GetScale(@scale, *M)

    If ( Not(scale\x = 0.0) And Not(scale\y = 0.0) And Not(scale\z = 0.0) And Not(scale\x = 1.0) Or Not(scale\y = 1.0) Or Not(scale\z = 1.0) )
        Vec3_Mul(@scale, @scale, @scale)
        Vec3_Div(@normal, @normal, @scale)
    EndIf

    Matrix_RotateVect(*M, @normal) 

    Vec3_Normalize(@normal)
    Plane_setPlane(*plane, @member,  @normal)
EndProcedure


Procedure Matrix_TranslateVect(*M.iMATRIX, *vect.iVECTOR3 )
		*vect\X = *vect\X+ *M\m[12]
		*vect\Y = *vect\Y+ *M\m[13]
		*vect\Z = *vect\Z+ *M\m[14]
EndProcedure


Procedure.l Matrix_GetInverse( *out.iMATRIX, *M.iMATRIX) 
		; Calculates the inverse of this Matrix
		; The inverse is calculated using Cramers rule.
		; If no inverse exists then 'false' is returned.

  	Protected mm.iMATRIX
  	Protected d.f
  	Matrix_Equals( @mm, *M)

		d = (mm\m[0] * mm\m[5] - mm\m[1] * mm\m[4]) * (mm\m[10] * mm\m[15] - mm\m[11] * mm\m[14]) -	(mm\m[0] * mm\m[6] - mm\m[2] * mm\m[4]) * (mm\m[9] * mm\m[15] - mm\m[11] * mm\m[13]) +	(mm\m[0] * mm\m[7] - mm\m[3] * mm\m[4]) * (mm\m[9] * mm\m[14] - mm\m[10] * mm\m[13]) +			(mm\m[1] * mm\m[6] - mm\m[2] * mm\m[5]) * (mm\m[8] * mm\m[15] - mm\m[11] * mm\m[12]) -	(mm\m[1] * mm\m[7] - mm\m[3] * mm\m[5]) * (mm\m[8] * mm\m[14] - mm\m[10] * mm\m[12]) +	(mm\m[2] * mm\m[7] - mm\m[3] * mm\m[6]) * (mm\m[8] * mm\m[13] - mm\m[9] * mm\m[12])

		If(  d =0 )
			ProcedureReturn 0 
    EndIf

		d = 1.0/ d 
				
		*out\m[0] = d * (mm\m[5] * (mm\m[10] * mm\m[15] - mm\m[11] * mm\m[14]) + mm\m[6] * (mm\m[11] * mm\m[13] - mm\m[9] * mm\m[15]) +  mm\m[7] * (mm\m[9] * mm\m[14] - mm\m[10] * mm\m[13]))
		*out\m[1] = d * (mm\m[9] * (mm\m[2] * mm\m[15] - mm\m[3] * mm\m[14]) +	mm\m[10] * (mm\m[3] * mm\m[13] - mm\m[1] * mm\m[15]) +	mm\m[11] * (mm\m[1] * mm\m[14] - mm\m[2] * mm\m[13]))
		*out\m[2] = d * (mm\m[13] * (mm\m[2] * mm\m[7] - mm\m[3] * mm\m[6]) + mm\m[14] * (mm\m[3] * mm\m[5] - mm\m[1] * mm\m[7]) +	mm\m[15] * (mm\m[1] * mm\m[6] - mm\m[2] * mm\m[5]))
		*out\m[3] = d * (mm\m[1] * (mm\m[7] * mm\m[10] - mm\m[6] * mm\m[11]) +		mm\m[2] * (mm\m[5] * mm\m[11] - mm\m[7] * mm\m[9]) +	mm\m[3] * (mm\m[6] * mm\m[9] - mm\m[5] * mm\m[10]))
		*out\m[4] = d * (mm\m[6] * (mm\m[8] * mm\m[15] - mm\m[11] * mm\m[12]) +	mm\m[7] * (mm\m[10] * mm\m[12] - mm\m[8] * mm\m[14]) +	mm\m[4] * (mm\m[11] * mm\m[14] - mm\m[10] * mm\m[15]))
		*out\m[5] = d * (mm\m[10] * (mm\m[0] * mm\m[15] - mm\m[3] * mm\m[12]) + mm\m[11] * (mm\m[2] * mm\m[12] - mm\m[0] * mm\m[14]) +	mm\m[8] * (mm\m[3] * mm\m[14] - mm\m[2] * mm\m[15]))
		*out\m[6] = d * (mm\m[14] * (mm\m[0] * mm\m[7] - mm\m[3] * mm\m[4]) + mm\m[15] * (mm\m[2] * mm\m[4] - mm\m[0] * mm\m[6]) +	mm\m[12] * (mm\m[3] * mm\m[6] - mm\m[2] * mm\m[7]))
		*out\m[7] = d * (mm\m[2] * (mm\m[7] * mm\m[8] - mm\m[4] * mm\m[11]) + mm\m[3] * (mm\m[4] * mm\m[10] - mm\m[6] * mm\m[8]) +	mm\m[0] * (mm\m[6] * mm\m[11] - mm\m[7] * mm\m[10]))
		*out\m[8] = d * (mm\m[7] * (mm\m[8] * mm\m[13] - mm\m[9] * mm\m[12]) +	mm\m[4] * (mm\m[9] * mm\m[15] - mm\m[11] * mm\m[13]) +	mm\m[5] * (mm\m[11] * mm\m[12] - mm\m[8] * mm\m[15]))
		*out\m[9] = d * (mm\m[11] * (mm\m[0] * mm\m[13] - mm\m[1] * mm\m[12]) +	mm\m[8] * (mm\m[1] * mm\m[15] - mm\m[3] * mm\m[13]) +	mm\m[9] * (mm\m[3] * mm\m[12] - mm\m[0] * mm\m[15]))
		*out\m[10] = d * (mm\m[15] * (mm\m[0] * mm\m[5] - mm\m[1] * mm\m[4]) +	mm\m[12] * (mm\m[1] * mm\m[7] - mm\m[3] * mm\m[5]) +	mm\m[13] * (mm\m[3] * mm\m[4] - mm\m[0] * mm\m[7]))
		*out\m[11] = d * (mm\m[3] * (mm\m[5] * mm\m[8] - mm\m[4] * mm\m[9]) + mm\m[0] * (mm\m[7] * mm\m[9] - mm\m[5] * mm\m[11]) +	mm\m[1] * (mm\m[4] * mm\m[11] - mm\m[7] * mm\m[8]))
		*out\m[12] = d * (mm\m[4] * (mm\m[10] * mm\m[13] - mm\m[9] * mm\m[14]) +	mm\m[5] * (mm\m[8] * mm\m[14] - mm\m[10] * mm\m[12]) +	mm\m[6] * (mm\m[9] * mm\m[12] - mm\m[8] * mm\m[13]))
		*out\m[13] = d * (mm\m[8] * (mm\m[2] * mm\m[13] - mm\m[1] * mm\m[14])+ mm\m[9] * (mm\m[0] * mm\m[14] - mm\m[2] * mm\m[12]) +	mm\m[10] * (mm\m[1] * mm\m[12] - mm\m[0] * mm\m[13]))
		*out\m[14] = d * (mm\m[12] * (mm\m[2] * mm\m[5] - mm\m[1] * mm\m[6]) +	mm\m[13] * (mm\m[0] * mm\m[6] - mm\m[2] * mm\m[4]) +	mm\m[14] * (mm\m[1] * mm\m[4] - mm\m[0] * mm\m[5]))
		*out\m[15] = d * (mm\m[0] * (mm\m[5] * mm\m[10] - mm\m[6] * mm\m[9]) +	mm\m[1] * (mm\m[6] * mm\m[8] - mm\m[4] * mm\m[10]) +	mm\m[2] * (mm\m[4] * mm\m[9] - mm\m[5] * mm\m[8]))
		
  ProcedureReturn 1
EndProcedure

Procedure.l Matrix_Transpose( *out.iMATRIX) 
 	Protected mm.iMATRIX
  	Matrix_Equals( @mm, *out)

		*out\m[ 0] = mm\m[ 0]
		*out\m[ 1] = mm\m[ 4]
		*out\m[ 2] = mm\m[ 8]
		*out\m[ 3] = mm\m[12]

		*out\m[ 4] = mm\m[ 1]
		*out\m[ 5] = mm\m[ 5]
		*out\m[ 6] = mm\m[ 9]
		*out\m[ 7] = mm\m[13]

		*out\m[ 8] = mm\m[ 2]
		*out\m[ 9] = mm\m[ 6]
		*out\m[10] = mm\m[10]
		*out\m[11] = mm\m[14]

		*out\m[12] = mm\m[ 3]
		*out\m[13] = mm\m[ 7]
		*out\m[14] = mm\m[11]
		*out\m[15] = mm\m[15]
EndProcedure


; 0   0,0
; 1   0,1
; 2   0,2
; 3   0,3
; 4   1,0
; 5   1,1
; 6   1,2
; 7   1,3
; 8   2,0
; 9   2,1
; 10  2,2
; 11  2,3
; 12  3,0
; 13  3,1
; 14  3,2
; 15  3,3
; IDE Options = PureBasic 4.30 Beta 5 (Windows - x86)
; CursorPosition = 249
; FirstLine = 246
; Folding = ----